import datetime
import numpy as np
import quadprog
from PyQt5 import QtCore,QtGui,QtWidgets

class AttackAngleController(QtWidgets.QWidget):
    _signal = QtCore.pyqtSignal(str)

    def __init__(self, parent=None):
        super(AttackAngleController, self).__init__(parent)

        # 动力学模型参数
        self.pi = 3.141592653
        self.dynamic_m1 = 2.4
        self.dynamic_m3 = 5.8
        self.dynamic_J2 = 0.02
        self.dynamic_kq2 = -0.29
        self.dynamic_g = 9.8
        self.dynamic_mb = 2.4
        self.dynamic_mm = 0.2
        self.dynamic_mw = 0.0
        self.dynamic_xp = 0.0
        self.dynamic_xb = -0.002
        self.dynamic_xm = 0.01
        self.dynamic_xw = 0.1
        self.dynamic_zb = 0.01
        self.dynamic_zm = -0.01
        self.dynamic_zw = 0.0
        self.dynamic_rx = self.dynamic_mb * self.dynamic_xb + self.dynamic_mm * self.dynamic_xm + self.dynamic_mw * self.dynamic_xw
        self.dynamic_rz = self.dynamic_mb * self.dynamic_zb + self.dynamic_mm * self.dynamic_zm + self.dynamic_mw * self.dynamic_zw
        self.dynamic_rz = 0.01
        
        self.m0 = 0.03 # 净浮力

        # backstepping参数-可调
        self.backstepping_c1 = 15
        self.backstepping_c2 = 40

        # MPC参数-可调
        self.mpc_Np = 20
        self.mpc_k = 1
        self.mpc_r = 0.000002

        # MPC参数-不可调
        self.mpc_u_ab = 90/180*self.pi
        self.mpc_u_re = 5/180*self.pi
        K = np.array([[1,-self.mpc_k],[-self.mpc_k,self.mpc_k**2]])
        self.mpc_Q_matrix = np.zeros((2*self.mpc_Np, 2*self.mpc_Np))
        for i in range(0, self.mpc_Np):
            self.mpc_Q_matrix[2*i:2*i+2, 2*i:2*i+2] = K
        self.mpc_R_matrix = np.diag(np.ones((self.mpc_Np,))*self.mpc_r)
        
        # 控制量
        self.ctl_ap = 0.0
        self.ctl_um = 0.0

        # Flag
        self.record_data_flag = False
        self.start_control_flag = False

        # 保存数据
        self.file_handle = None

        # 初始化UI
        self.init_ui()

    # 初始化UI界面
    def init_ui(self):
        # 窗口设置
        self.setFixedSize(220, 480)  # 设置窗体大小
        self.setWindowTitle('攻角控制器')  # 设置窗口标题

        # 控件初始化
        self.status_label = QtWidgets.QLabel('系统状态')
        self.status_label.setFont(QtGui.QFont('Microsoft YaHei', 10, QtGui.QFont.Bold))
        self.angle_fixed_label = QtWidgets.QLabel('角度')
        self.angle_data_label = QtWidgets.QLabel('0.0')
        self.angvel_fixed_label = QtWidgets.QLabel('角速度')
        self.angvel_data_label = QtWidgets.QLabel('0.0')
        self.velX_fixed_label = QtWidgets.QLabel('X速度')
        self.velX_data_label = QtWidgets.QLabel('0.0')
        self.velY_fixed_label = QtWidgets.QLabel('Y速度')
        self.velY_data_label = QtWidgets.QLabel('0.0')
        self.ctlA_fixed_label = QtWidgets.QLabel('控制量A')
        self.ctlA_data_label = QtWidgets.QLabel('0.0')
        self.ctlB_fixed_label = QtWidgets.QLabel('控制量B')
        self.ctlB_data_label = QtWidgets.QLabel('0.0')

        self.backstepping_param_label = QtWidgets.QLabel('Backstepping参数')
        self.backstepping_param_label.setFont(QtGui.QFont('Microsoft YaHei', 10, QtGui.QFont.Bold))
        self.backstepping_param_c1_label = QtWidgets.QLabel('C1('+str(self.backstepping_c1)+')')
        self.backstepping_param_c1_editor = QtWidgets.QLineEdit()
        self.backstepping_param_c1_editor.setText(str(self.backstepping_c1))
        self.backstepping_param_c2_label = QtWidgets.QLabel('C2('+str(self.backstepping_c2)+')')
        self.backstepping_param_c2_editor = QtWidgets.QLineEdit()
        self.backstepping_param_c2_editor.setText(str(self.backstepping_c2))

        self.mpc_param_label = QtWidgets.QLabel('MPC参数')
        self.mpc_param_label.setFont(QtGui.QFont('Microsoft YaHei', 10, QtGui.QFont.Bold))
        self.mpc_param_Np_label = QtWidgets.QLabel('Np('+str(self.mpc_Np)+')')
        self.mpc_param_Np_editor = QtWidgets.QLineEdit()
        self.mpc_param_Np_editor.setText(str(self.mpc_Np))
        self.mpc_param_k_label = QtWidgets.QLabel('k('+str(self.mpc_k)+')')
        self.mpc_param_k_editor = QtWidgets.QLineEdit()
        self.mpc_param_k_editor.setText(str(self.mpc_k))
        self.mpc_param_r_label = QtWidgets.QLabel('r('+str(self.mpc_r)+')')
        self.mpc_param_r_editor = QtWidgets.QLineEdit()
        self.mpc_param_r_editor.setText(str(self.mpc_r))

        self.button_label = QtWidgets.QLabel('按钮')
        self.button_label.setFont(QtGui.QFont('Microsoft YaHei', 10, QtGui.QFont.Bold))
        self.write_param_button = QtWidgets.QPushButton('写入参数')
        self.start_control_button = QtWidgets.QPushButton('开始控制')
        self.record_data_button = QtWidgets.QPushButton('记录数据')
        self.stop_control_button = QtWidgets.QPushButton('停止控制')

        # 布局
        self.main_layout = QtWidgets.QGridLayout()
        self.setLayout(self.main_layout)
        self.main_layout.addWidget(self.status_label, 0, 0, 1, 2, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.angle_fixed_label, 1, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.angle_data_label, 1, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.angvel_fixed_label, 2, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.angvel_data_label, 2, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.velX_fixed_label, 3, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.velX_data_label, 3, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.velY_fixed_label, 4, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.velY_data_label, 4, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.ctlA_fixed_label, 5, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.ctlA_data_label, 5, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.ctlB_fixed_label, 6, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.ctlB_data_label, 6, 1, 1, 1, QtCore.Qt.AlignLeft)

        self.main_layout.addWidget(self.backstepping_param_label, 7, 0, 1, 2, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.backstepping_param_c1_label, 8, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.backstepping_param_c1_editor, 8, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.backstepping_param_c2_label, 9, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.backstepping_param_c2_editor, 9, 1, 1, 1, QtCore.Qt.AlignLeft)

        self.main_layout.addWidget(self.mpc_param_label, 10, 0, 1, 2, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.mpc_param_Np_label, 11, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.mpc_param_Np_editor, 11, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.mpc_param_k_label, 12, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.mpc_param_k_editor, 12, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.mpc_param_r_label, 13, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.mpc_param_r_editor, 13, 1, 1, 1, QtCore.Qt.AlignLeft)

        self.main_layout.addWidget(self.button_label, 14, 0, 1, 2, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.write_param_button, 15, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.start_control_button, 15, 1, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.record_data_button, 16, 0, 1, 1, QtCore.Qt.AlignLeft)
        self.main_layout.addWidget(self.stop_control_button, 16, 1, 1, 1, QtCore.Qt.AlignLeft)

        # 链接控件
        self.write_param_button.clicked.connect(self.write_param_button_slot)
        self.start_control_button.clicked.connect(self.start_control_button_slot)
        self.stop_control_button.clicked.connect(self.stop_control_button_slot)
        self.record_data_button.clicked.connect(self.record_data_button_slot)

    # 槽函数
    def write_param_button_slot(self):
        try:
            self.backstepping_c1 = float(self.backstepping_param_c1_editor.text())
            self.backstepping_c2 = float(self.backstepping_param_c2_editor.text())
            self.mpc_Np = int(self.mpc_param_Np_editor.text())
            self.mpc_k = float(self.mpc_param_k_editor.text())
            self.mpc_r = float(self.mpc_param_r_editor.text())
            K = np.array([[1,-self.mpc_k],[-self.mpc_k,self.mpc_k**2]])
            self.mpc_Q_matrix = np.zeros((2*self.mpc_Np, 2*self.mpc_Np))
            for i in range(0, self.mpc_Np):
                self.mpc_Q_matrix[2*i:2*i+2, 2*i:2*i+2] = K
            self.mpc_R_matrix = np.diag(np.ones((self.mpc_Np,))*self.mpc_r)

            self.backstepping_param_c1_label.setText('C1('+str(self.backstepping_c1)+')') 
            self.backstepping_param_c2_label.setText('C2('+str(self.backstepping_c2)+')')
            self.mpc_param_Np_label.setText('Np('+str(self.mpc_Np)+')')
            self.mpc_param_k_label.setText('k('+str(self.mpc_k)+')')
            self.mpc_param_r_label.setText('r('+str(self.mpc_r)+')')
        except ValueError:
            print('Controller param must be float number')

    # 槽函数
    def start_control_button_slot(self):
        self.start_control_flag = True
    
    # 槽函数
    def stop_control_button_slot(self):
        self.start_control_flag = False

    # 槽函数
    def record_data_button_slot(self):
        self.record_data_flag = not self.record_data_flag
        if self.record_data_flag:
            self.record_data_button.setText('停止记录')
            now_time = datetime.datetime.now()
            filename = 'data/controller_' + str(now_time.year-2000).zfill(2) + str(now_time.month).zfill(2) + str(now_time.day).zfill(2) + \
                        str(now_time.hour).zfill(2) + str(now_time.minute).zfill(2) + str(now_time.second).zfill(2) + '.txt'
            self.file_handle = open(filename, 'w')
        else:
            self.record_data_button.setText('记录数据')
            if self.file_handle is not None:
                self.file_handle.close()

    # 开启界面
    def handle_click(self):
        if not self.isVisible():
            self.show()

    # 关闭界面
    def handle_close(self):
        self.close()

    # 控制主函数，输入是检测到的状态，输出是控制指令
    def control(self, time, dt, angle, angvel, velX, velY):
        # 显示系统状态
        self.angle_data_label.setText(('%.3f' % angle))
        self.angvel_data_label.setText(('%.3f' % angvel))
        self.velX_data_label.setText(('%.3f' % velX))
        self.velY_data_label.setText(('%.3f' % velY))
        # 是否开启控制
        if self.start_control_flag:
            # 读取已知信息
            ts = dt
            v1 = velX
            v3 = velY
            w2 = angvel
            theta = angle

            a = np.arccos(v1/np.sqrt(v1**2+v3**2))
            ap = self.ctl_ap
            um = self.ctl_um

            # 计算身体水动力参数
            CD = fcn_HDb(a)
            CL = fcn_HLb(a)
            CM2 = fcn_HM2b(a)
            D = CD*(v1**2+v3**2)
            L = CL*(v1**2+v3**2)
            M2 = CM2*(v1**2+v3**2)+self.dynamic_kq2*w2

            # 计算胸鳍水动力参数
            vp2 = v1**2+(v3-self.dynamic_xp*w2)**2
            CLp = fcn_HLp(ap)
            CDp = fcn_HDp(ap)
            Dp = CDp*vp2
            Lp = CLp*vp2
            f1u = -Dp*np.cos(a)+Lp*np.sin(a)
            f3u = -Dp*np.sin(a)-Lp*np.cos(a)
            M2u = -f3u*self.dynamic_xp

            # Backstepping控制器
            f = ((self.dynamic_m3-self.dynamic_m1)*v1*v3 + M2 + M2u - self.dynamic_rz*v3*w2 - self.dynamic_rz*self.dynamic_g*np.sin(theta))/self.dynamic_J2
            b = -(self.dynamic_mm*v1*w2 + self.dynamic_mm*self.dynamic_g*np.cos(theta))/self.dynamic_J2   
            um0 = (-f - (self.backstepping_c1+self.backstepping_c2)*w2 - (1 + self.backstepping_c1*self.backstepping_c2 + self.backstepping_c1**2)*theta)/b
            um0 = max(um0, -0.04)
            um0 = min(um0, 0.04)

            # MPC控制器
            A, B0, C = fcn_linerize(v1, v3, ap, um, self.m0)
            B = B0[:, 0].reshape(2,1)
            A1 = A*ts + np.diag([1,1])
            B1 = B*ts
            C1 = C*ts
            Gamma = np.zeros((2*self.mpc_Np,2))
            Gamma[0:2,0:2] = A1
            Psi = np.zeros((2*self.mpc_Np, 1*self.mpc_Np))
            Psi[0:2, 0:1] = B1
            Omega = np.zeros((2*self.mpc_Np, 1))
            Omega[0:2, 0:1] = C1
            A1j = A1
            A1Power = []
            A1Power.append(A1j)
            for i in range(1, self.mpc_Np):
                C1j = C1 + np.dot(A1j,C1)
                A1j = np.dot(A1j, A1)
                A1Power.append(A1j)
                Gamma[2*i:2*i+2, 0:2] = A1j
                Omega[2*i:2*i+2, 0:1] = C1j
                for k in range(0, i):
                    B1j = np.dot(A1Power[i-k-1], B1)
                    Psi[2*i:2*i+2, k:k+1] = B1j

            # MPC控制器-构造优化变量
            X0 = np.array([[v1],[v3]])
            Xhat = np.tile(X0, (self.mpc_Np,1))

            # MPC控制器-构造约束
            LB = max(-self.mpc_u_ab-ap, -self.mpc_u_re)
            UB = min(self.mpc_u_ab-ap, self.mpc_u_re)
            ulb = np.tile(LB, (self.mpc_Np,1))
            uub = np.tile(UB, (self.mpc_Np,1))
            constraint_A = np.vstack([np.eye(self.mpc_Np), -np.eye(self.mpc_Np)]).transpose()
            constraint_B = np.vstack([ulb, -uub])
            constraint_B = constraint_B.reshape(-1,)

            # MPC控制器-滚动优化
            H = np.dot(np.dot(Psi.transpose(), self.mpc_Q_matrix), Psi) + self.mpc_R_matrix
            H = (H+H.transpose())/2
            f = np.dot(np.dot((Xhat+Omega).transpose(), self.mpc_Q_matrix), Psi).transpose()
            f = f.reshape(-1,)
            try:
                solution = quadprog.solve_qp(H, -f, constraint_A, constraint_B, 0) 
                du1 = solution[0][0] # 获得控制量
                ap = ap + du1
            except ValueError:
                print("Quadprog Error")

            # 显示控制量
            self.ctl_um = um0
            self.ctlA_data_label.setText(('%.3f' % self.ctl_um))
            self.ctl_ap = ap
            self.ctlB_data_label.setText(('%.3f' % self.ctl_ap))

        if self.record_data_flag:
            datastr = ('%.2f' % time) + ' ' + ('%.6f' % angle) + ' ' + ('%.6f' % angvel) + ' ' + ('%.6f' % velX) + ' ' + ('%.6f' % velY) + \
                      ' ' + ('%.6f' % self.ctl_um) + ' ' + ('%.6f' % self.ctl_ap) + '\n'
            self.file_handle.write(datastr)

        if self.start_control_flag:
            return int(self.ctl_um*1000), int((self.ctl_ap-a)/self.pi*180)
        else:
            return 0, 0


#####################################################################################################################################################
def fcn_linerize(x1, x2, a, u1, u2):
    m1 = 2.4
    m2 = 5.8
    g = 9.8
    HDb = fcn_HDb(a)
    HLb = fcn_HLb(a)
    HDp = fcn_HDp(u1)
    HLp = fcn_HLp(u1)
    dHDb = fcn_dHDb(a)
    dHLb = fcn_dHLb(a)
    dHDp = fcn_dHDp(u1)
    dHLp = fcn_dHLp(u1)
    v = np.sqrt(x1**2+x2**2)
    f1x1 = x1/m1/v*(-(HDb+HDp)*x1+(HLb+HLp)*x2)+v/m1*(-HDb-HDp+dHDb*x1*x2/v/v-dHLb*x2*x2/v/v)
    f1x2 = x2/m1/v*(-(HDb+HDp)*x1+(HLb+HLp)*x2)+v/m1*(HLb+HLp-dHDb*x1*x1/v/v+dHLb*x1*x2/v/v)
    f2x1 = x1/m2/v*(-(HLb+HLp)*x1-(HDb+HDp)*x2)+v/m2*(-HLb-HLp+dHDb*x1*x1/v/v+dHLb*x1*x2/v/v)
    f2x2 = x2/m2/v*(-(HLb+HLp)*x1-(HDb+HDp)*x2)+v/m2*(-HDb-HDp-dHDb*x1*x2/v/v-dHLb*x2*x2/v/v)
    f1u1 = v/m1*(-dHDp*x1+dHLp*x2)
    f1u2 = 0
    f2u1 = v/m2*(-dHLp*x1-dHLp*x2)
    f2u2 = -g/m2
    c1 = 1/m1*v*(-(HDb+HDp)*x1+(HLb+HLp)*x2)
    c2 = 1/m2*v*(-(HLb+HLp)*x1-(HDb+HDp)*x2)-g*u2/m2
    A = np.array([[f1x1,f1x2],[f2x1,f2x2]])
    B = np.array([[f1u1,f1u2],[f2u1,f2u2]])
    C = np.array([[c1],[c2]])
    return A, B, C

def fcn_HDb(x):
    k = np.array([0.03892, -0.45292,1.966,-3.69,2.202,0.7402, -0.01398,0.1542,0.06812]).reshape(1,-1)
    a = np.array([x**8, x**7*np.sign(x), x**6, x**5*np.sign(x), x**4, x**3*np.sign(x), x**2, x*np.sign(x), 1]).reshape(-1,1)
    Db = np.dot(k, a)
    D = Db[0,0]*11
    return D

def fcn_HLb(x):
    k = np.array([-0.100,1.3411,-7.252,19.91,-28.41,19.07,-5.06,1.128,-0.0004272]).reshape(1,-1)
    a = np.array([x**8*np.sign(x), x**7, x**6*np.sign(x), x**5, x**4*np.sign(x), x**3, x**2*np.sign(x), x, np.sign(x)]).reshape(-1,1)
    Lb = np.dot(k, a)
    L = Lb[0,0]*11
    return L

def fcn_HM2b(x):
    k = np.array([0.002273,-0.03092,0.1675,-0.4457,0.5842,-0.4224,0.4022,-0.1912,0.0001836]).reshape(1,-1)
    a = np.array([x**8*np.sign(x), x**7, x**6*np.sign(x), x**5, x**4*np.sign(x), x**3, x**2*np.sign(x), x, np.sign(x)]).reshape(-1,1)
    m2f = np.dot(k, a)
    M2 = m2f[0,0]*11
    return M2

def fcn_HLp(x):
    type=34
    if type == 22:
        dLp = -0.04391*x**4*np.sign(x) + 0.1943*x**3 - 0.3939*x**2*np.sign(x) + 0.3113*x
    elif type == 23:
        dLp = 0.1756*x**5 - 0.7935*x**4*np.sign(x) +  1.3632*x**3 - 1.249*x**2*np.sign(x) + 0.6091*x
    elif type == 24:
        dLp =  0.3511*x**5 - 1.543*x**4*np.sign(x) + 2.532*x**3 - 2.104*x**2*np.sign(x) + 0.9069*x
    elif type == 33:
        dLp = -0.09632*x**4*np.sign(x) + 0.4091*x**3 - 0.8325*x**2*np.sign(x) + 0.675*x
    elif type == 34:
        dLp = -0.2079*x**4*np.sign(x) + 0.8132*x**3 - 1.393*x**2*np.sign(x) + 0.9906*x
    elif type == 43:
        dLp = 0.08842*x**4*np.sign(x) -0.1611*x**3 - 0.3947*x**2*np.sign(x) + 0.678*x
    dHLp=dLp*22
    return dHLp

def fcn_HDp(x):
    type = 34
    if type == 22:
        Dp = 0.01672*x**4 - 0.1074*x**3*np.sign(x) + 0.1761*x**2 + 0.02102*x*np.sign(x) + 0.004492
    elif type == 23:
        Dp = -0.0083*x**4 - 0.0762*x**3*np.sign(x) +  0.1963*x**2 +  0.0526*x*np.sign(x) + 0.009
    elif type == 24:
        Dp = -0.0333*x**4 - 0.04501*x**3*np.sign(x) + 0.2165*x**2 + 0.08414*x*np.sign(x) + 0.01474
    elif type == 33:
        Dp = 0.03066*x**4 - 0.2381*x**3*np.sign(x) + 0.4225*x**2 + 0.02922*x*np.sign(x) + 0.00971
    elif type == 34:
        Dp = 0.02575*x**4 - 0.2683*x**3*np.sign(x) + 0.5112*x**2 + 0.06215*x*np.sign(x) + 0.01449
    elif type == 43:
        Dp = 0.05871*x**4 - 0.4366*x**3*np.sign(x) + 0.7324*x**2 - 0.02489*x*np.sign(x) + 0.01073
    HDp = Dp*22
    return HDp

def fcn_dHDb(x):
    k = np.array([8, 7, 6, 5, 4, 3, 2, 1, 0]).reshape(1,-1)
    a1 = np.array([x**7, x**6, x**5, x**4, x**3, x**2, x, 1, 0]).reshape(-1,1)
    dDb = np.dot(np.array([0.03892, -0.45292,1.966,-3.69,2.202,0.7402, -0.01398,0.1542,0.06812]).reshape(1,-1)*k, a1)
    dD = dDb[0,0]*11
    return dD

def fcn_dHLb(x):
    k = np.array([8, 7, 6, 5, 4, 3, 2, 1, 0]).reshape(1,-1)
    a1 = np.array([x**7, x**6, x**5, x**4, x**3, x**2, x, 1, 0]).reshape(-1,1)
    dLb = np.dot(np.array([-0.100,1.3411,-7.252,19.91,-28.41,19.07,-5.06,1.128,-0.0004272]).reshape(1,-1)*k, a1)
    dL=dLb[0,0]*11
    return dL

def fcn_dHDp(x):
    type = 34
    if type == 22:
        dDp= 0.01672*x**3*4 -0.1074 *x**2*3 * np.sign(x)+ 0.1761 *x*2 + 0.02102  * np.sign(x)
    elif type == 23:
        dDp = -0.0083*x**3*4 -0.0762 *x**2*3 * np.sign(x)+  0.1963*x*2 +  0.0526 * np.sign(x)
    elif type == 24:
        dDp = -0.0333*x**3*4 -0.04501 *x**2*3 * np.sign(x)+ 0.2165 *x*2 + 0.08414  * np.sign(x)
    elif type == 33:
        dDp = 0.03066*x**3*4 -0.2381 *x**2*3 * np.sign(x)+ 0.4225 *x*2 + 0.02922  * np.sign(x)
    elif type == 34:
        dDp = 0.02575*x**3*4 -0.2683 *x**2*3 * np.sign(x)+ 0.5112 *x*2 + 0.06215  * np.sign(x)
    elif type == 43:
        dDp = 0.05871*x**3*4 -0.4366 *x**2*3 * np.sign(x)+ 0.7324 *x*2 - 0.02489  * np.sign(x)
    dHDp=dDp*22
    return dHDp

def fcn_dHLp(x):
    type=34
    if type == 22:
        dLp = -0.04391*x**3*4*np.sign(x) + 0.1943*x**2*3 - 0.3939*x*2*np.sign(x) + 0.3113
    elif type == 23:
        dLp = 0.1756*x**4*5 - 0.7935*x**3*4*np.sign(x) +  1.3632*x**2*3 - 1.249*x*2*np.sign(x) + 0.6091
    elif type == 24:
        dLp =  0.3511*x**4*5 - 1.543*x**3*4*np.sign(x) + 2.532*x**2*3 - 2.104*x*2*np.sign(x) + 0.9069
    elif type == 33:
        dLp = -0.09632*x**3*4*np.sign(x) + 0.4091*x**2*3 - 0.8325*x*2*np.sign(x) + 0.675
    elif type == 34:
        dLp = -0.2079*x**3*4*np.sign(x) + 0.8132*x**2*3 - 1.393*x*2*np.sign(x) + 0.9906
    elif type == 43:
        dLp = 0.08842*x**3*4*np.sign(x) -0.1611*x**2*3 - 0.3947*x*2*np.sign(x) + 0.678
    dHLp=dLp*22
    return dHLp


def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None):
    # 问题：min 0.5*x*P*x - qx
    # G是不等式约束 Gx>=h
    # A是等式约束 Ax=b
    qp_G = .5 * (P + P.T)   # make sure P is symmetric
    qp_a = -q
    if A is not None:
        qp_C = -np.vstack([A, G]).T
        qp_b = -np.hstack([b, h])
        meq = A.shape[0]
    else:  # no equality constraint
        qp_C = -G.T
        qp_b = -h
        meq = 0
    return quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0] 